#include "mainwindow.h"
#include <qt4/QtCore/QVector>
#include <qt4/Qt/qtimer.h>

#include <qt4/Qt/qcolor.h>

#include <qt4/QtCore/QObject>
#include <qt4/QtGui/QWidget>
#include <qt4/QtGui/QColor>
#include <qt4/QtGui/qmainwindow.h>
#include <qt4/QtGui/qpen.h>
#include <qt4/QtCore/QCoreApplication>
#include <qt4/QtGui/qpushbutton.h>
#include <qwt_plot.h>
#include <qwt_plot_curve.h>  
#include <qwt_plot_magnifier.h>  
#include <qwt_plot_panner.h>  
#include <qwt_legend.h>  
#include <qwt_series_data.h>
#include <opencv2/opencv.hpp>

#include <fstream>

mainwindow::mainwindow(QWidget *parent)
: QMainWindow(parent)
{
    //首先初始化，构造相应的对象
    setupUi(this);

    _qwtdatanumber = 500;

    //设置界面
    _dataqwtplot->setTitle("Gyroscope Z axis value");
    _dataqwtplot->setAxisScale(QwtPlot::xBottom,  0,  _qwtdatanumber);
    _dataqwtplot_acc->setAxisScale(QwtPlot::xBottom,  0, _qwtdatanumber);

    //初始化相关参数
    _readbtnpushed = false;
    _sensorcalibrated = false;
    calibrated = false;

    _readtimer = new QTimer();
    _displaytimer = new QTimer();

    makerbinocular_ = new makerbinocular();

    //存储相关的数据
    _sensorframegot = 0;
    _sensordata = new double[6];
    last_update_index = 0;

    imu_frame_get = 0;
    sampling = false;

    x_gyro = new double[_qwtdatanumber];
    y_gyro = new double[_qwtdatanumber];
    z_gyro = new double[_qwtdatanumber];
    x_acc = new double[_qwtdatanumber];
    y_acc = new double[_qwtdatanumber];
    z_acc = new double[_qwtdatanumber];

    const int plane_numbers = 6;
    const int axis_numbers = 3;

    //acc_sample = new double*[plane_numbers]; 

    //for (int i = 0; i < plane_numbers; i++)
    //acc_sample[i] = new double[3];

    acc = new float[12];
    gyro = new float[12];
    
    for (int i = 0; i < 6; i++)
        for (int j = 0; j < 3; j++)
            acc_sample[i][j] = 0; 
    

    //信号与回调函数连接
    connect(_startbutton, SIGNAL(released()), this, SLOT(readbtntoggled()));
    connect(_endbutton, SIGNAL(released()), this, SLOT(stopbtntoggled()));

    //timer回调函数
    QObject::connect(_readtimer, SIGNAL(timeout()), this, SLOT(read_data()));
    QObject::connect(_displaytimer, SIGNAL(timeout()), this, SLOT(update_ui()));

    //timer
    //_readtimer->setInterval(30);
    _readtimer->start(30);

    //_displaytimer->setInterval(30);
    _displaytimer->start(30);

}

void mainwindow::update_ui()
{
    refreshqwtplot();
}

/**
 * @brief ...
 * 
 * @return void
 */
void mainwindow::read_data()
{
    float camera_interval_ms;
    float imu_interval_ms[4];
    if (makerbinocular_->is_initialized())
    {
        cv::Mat left_image(480, 640, CV_8UC1, cv::Scalar(0));
        cv::Mat right_image(480, 640, CV_8UC1, cv::Scalar(0));

        float acc_new_frame[3], gyro_new_frame[3];

        if (makerbinocular_->get_frame(left_image,  right_image, acc, gyro, camera_interval_ms, imu_interval_ms))
        {
			imshow("left image", left_image);
			cv::waitKey(1);

			imshow("right image", right_image);
			cv::waitKey(1);

            if (_readbtnpushed  || calibrated)
            {
                    for (int i = 0; i < 4; i++)
                    {
                        int _currentindex = _sensorframegot % _qwtdatanumber;
                        // 为了显示
                        x_gyro[_currentindex] = gyro[0 + 3 * i];
                        y_gyro[_currentindex] = gyro[1 + 3 * i];
                        z_gyro[_currentindex] = gyro[2 + 3 * i];
                        x_acc[_currentindex] = acc[0 + 3 * i];
                        y_acc[_currentindex] = acc[1 + 3 * i];
                        z_acc[_currentindex] = acc[2 + 3 * i];

                        // 求均值，用于标定
                        acc_new_frame[0] = acc[0 + 3 * i];
                        acc_new_frame[1] = acc[1 + 3 * i];
                        acc_new_frame[2] = acc[2 + 3 * i];

                        gyro_new_frame[0] = gyro[0 + 3 * i];
                        gyro_new_frame[1] = gyro[1 + 3 * i];
                        gyro_new_frame[2] = gyro[2 + 3 * i];
                        
                        double calibrated_acc_value[3];
                        double acc_raw_data[3];
                        acc_raw_data[0] = acc[0 + 3 *i ];
                        acc_raw_data[1] = acc[1 + 3 * i];
                        acc_raw_data[2] = acc[2 + 3 * i];
                        
                        cv::Mat acc_raw_v(3, 1, CV_64F, acc_raw_data);
                        cv::Mat c_acc_v(3, 1, CV_64F, calibrated_acc_value);
                        cv::Mat K_a(3, 3, CV_64F,  k_a);
                        cv::Mat a_bias(3, 1, CV_64F, b_a);
                        
                        if (calibrated)
                        {
                            c_acc_v = K_a.inv() * (acc_raw_v - a_bias);
                            
                            std::cout << calibrated_acc_value[0] <<  " " << 
                              calibrated_acc_value[1] <<  " " <<  calibrated_acc_value[2] <<  std::endl;
                        }

                        _sensorframegot++;

                        if (imu_frame_get ==  0)
                        {
                            // 判断当前的重力的方向
                            if (fabs(x_acc[_currentindex]) < fabs(y_acc[_currentindex]))
                            {
                                if (fabs(y_acc[_currentindex]) < fabs(z_acc[_currentindex]))
                                {
                                    // -z
                                    if (z_acc[_currentindex] < 0)
                                    index = 5;
                                    else
                                    index = 4;              // +z
                                }
                                else
                                {
                                    // -y
                                    if (y_acc[_currentindex] < 0)
                                    index = 3;
                                    else
                                    index = 2;              // y
                                }
                            }
                            else
                            {
                                if (fabs(x_acc[_currentindex]) < fabs(z_acc[_currentindex]))
                                {
                                    // -z
                                    if (z_acc[_currentindex] < 0)
                                    index = 5;
                                    else
                                    index = 4;              // z
                                }
                                else
                                {
                                    // -x
                                    if (x_acc[_currentindex] < 0)
                                    index = 1;
                                    else                    // x
                                    index = 0; 
                                }
                            }
                        }

                        if (sampling == true && !(calibrated))
                        {  
                            add_new_imu_sample(acc_new_frame, gyro_new_frame);
                            imu_frame_get++;

                            std::cout <<  imu_frame_get <<  std::endl;

                            if (imu_frame_get >=  100)
                            {
                                //    _readbtnpushed = false;
                                imu_frame_get = 0;
                                sampling = false;
                                
                                std::cout << index <<  std::endl;
                                std::cout << acc_sample[0][index] <<  acc_sample[1][index] <<  
                                  acc_sample[2][index] <<  std::endl;
                                  
                                sampled_plane[index] = true;
                                
                                if (checksampleddata())
                                {
                                    calibrateimu();
                                    calibrated = true;
                                }
                            }
                        }
                    }
            }
        }
    }
}

void mainwindow::add_new_imu_sample(float new_acc[3], float new_gyro[3])
{
    acc_sample[0][index] = (acc_sample[0][index] * imu_frame_get + new_acc[0]) / (imu_frame_get + 1);
    acc_sample[1][index] = (acc_sample[1][index] * imu_frame_get + new_acc[1]) / (imu_frame_get + 1);
    acc_sample[2][index] = (acc_sample[2][index] * imu_frame_get + new_acc[2]) / (imu_frame_get + 1);
}

void mainwindow::calibrateimu()
{
    cv::Mat U(3, 6, CV_64F, acc_sample);
    const double gravity = 9.8; 
    double a_array[4][6] = {{gravity, -gravity, 0, 0, 0, 0}, {0, 0, gravity, -gravity, 0, 0}, {0, 0, 0, 0, gravity, -gravity}, 
    {1, 1, 1, 1, 1, 1}};
    
    double k[3][4];
    cv::Mat A(4, 6, CV_64F, a_array);
    
    cv::Mat temp = A * A.t();
    
    cv::Mat K(3, 4, CV_64F, k);
    
    K = U * A.t() * temp.inv();
    
    std::ofstream out;
    
    out.open("IMU_Intrinsic.txt", std::ios::out);
    
    out <<  "Acc Intrinsic: k_a" <<  std::endl;
    
    for ( int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            k_a[i][j] = k[i][j];
            out <<  k[i][j] << " ";
        }
        out <<  std::endl;   
    }
       
    out <<  "Acc Intrinsic: bias" <<  std::endl;
    
    for (int i = 0; i < 3; i++)
    {
        out <<  k[i][3] << " ";
        b_a[i] = k[i][3];
    }
    out <<  std::endl;
    out.close();
    
    std::cout << K <<  std::endl;
    
}

bool mainwindow::checksampleddata()
{
    const int plane_number = 6;
    for (int i = 0 ; i < plane_number; i++)
    {
        if (sampled_plane[i] != true)
            return false;
    }
    
    return true;
}

void mainwindow::calibrate_acc()
{

// calibrate the acc 
// 六面法
    

}

void mainwindow::readbtntoggled()
{
    _readbtnpushed = true;
    sampling = true;
    std::cout << "--- read button pushed!" << std::endl;
}

void mainwindow::stopbtntoggled()
{
    _readbtnpushed = false;
    std::cout << "--- stop button pushed!" << std::endl;
}

void mainwindow::refreshqwtplot()
{
    _dataqwtplot->replot();
    int _startindex = _sensorframegot % _qwtdatanumber;

    QwtPlotCurve * x_axis_curve = new QwtPlotCurve();
    QwtPlotCurve * y_axis_curve = new QwtPlotCurve();
    QwtPlotCurve * z_axis_curve = new QwtPlotCurve();
    QwtPlotCurve * x_acc_curve = new QwtPlotCurve();
    QwtPlotCurve * y_acc_curve = new QwtPlotCurve();
    QwtPlotCurve * z_acc_curve = new QwtPlotCurve();

    for ( int i = last_update_index; i < _startindex; i++)
    {
        gyro_x_axis <<  QPointF(i, x_gyro[i]);
        gyro_y_axis <<  QPointF(i, y_gyro[i]);
        gyro_z_axis <<  QPointF(i, z_gyro[i]);

        acc_x_axis <<  QPointF(i,  x_acc[i]);
        acc_y_axis <<  QPointF(i,  y_acc[i]);
        acc_z_axis <<  QPointF(i,  z_acc[i]);
    }

    x_axis_curve->setSamples(gyro_x_axis);
    x_axis_curve->setPen(QPen(Qt::red));
    y_axis_curve->setSamples(gyro_y_axis);
    y_axis_curve->setPen(QPen(Qt::green));
    z_axis_curve->setSamples(gyro_z_axis);
    z_axis_curve->setPen(QPen(Qt::blue));

    x_acc_curve->setSamples(acc_x_axis);
    x_acc_curve->setPen(QPen(Qt::red));
    y_acc_curve->setSamples(acc_y_axis);
    y_acc_curve->setPen(QPen(Qt::green));
    z_acc_curve->setSamples(acc_z_axis);
    z_acc_curve->setPen(QPen(Qt::blue));

    x_axis_curve->attach(_dataqwtplot);
    y_axis_curve->attach(_dataqwtplot);
    z_axis_curve->attach(_dataqwtplot);

    x_acc_curve->attach(_dataqwtplot_acc);
    y_acc_curve->attach(_dataqwtplot_acc);
    z_acc_curve->attach(_dataqwtplot_acc);

    if (sampling)
    {
        x_axis_curve->setPen(QPen(Qt::yellow));
        y_axis_curve->setPen(QPen(Qt::yellow));
        z_axis_curve->setPen(QPen(Qt::yellow));

        // 
        x_acc_curve->setPen(QPen(Qt::yellow));
        y_acc_curve->setPen(QPen(Qt::yellow));
        z_acc_curve->setPen(QPen(Qt::yellow));
    }

    _dataqwtplot->replot();
    _dataqwtplot_acc->replot();

    if (_startindex < last_update_index) 
    {
        gyro_x_axis.clear();
        gyro_y_axis.clear();
        gyro_z_axis.clear();

        acc_x_axis.clear();
        acc_y_axis.clear();
        acc_z_axis.clear(); 
        
        _dataqwtplot->detachItems(QwtPlotItem::Rtti_PlotItem, true);
        _dataqwtplot_acc->detachItems(QwtPlotItem::Rtti_PlotItem, true);

    }

    last_update_index = _startindex;
}
